package structs;

public class Matrix {

	private float m00;
	private float m01;
	private float m02;
	private float m03;
	private float m10;
	private float m11;
	private float m12;
	private float m13;
	private float m20;
	private float m21;
	private float m22;
	private float m23;
	private float m30;
	private float m31;
	private float m32;
	private float m33;

	public Matrix() {
		// All-zero matrix
	}

	public Matrix(float[][] data) {
		if (data.length != 4 || data[1].length != 4)
			throw new IllegalArgumentException();

		m00 = data[0][0];
		m01 = data[0][1];
		m02 = data[0][2];
		m03 = data[0][3];
		m10 = data[1][0];
		m11 = data[1][1];
		m12 = data[1][2];
		m13 = data[1][3];
		m20 = data[2][0];
		m21 = data[2][1];
		m22 = data[2][2];
		m23 = data[2][3];
		m30 = data[3][0];
		m31 = data[3][1];
		m32 = data[3][2];
		m33 = data[3][3];
	}

	public static Matrix identity() {
		float[][] data = new float[4][4];
		for (int i = 0; i < 4; i++) {
			data[i][i] = 1;
		}
		return new Matrix(data);
	}

	public static Matrix translation(Vector3 translation) {
		float[][] data = new float[4][4];
		for (int i = 0; i < 4; i++) {
			data[i][i] = 1;
		}
		data[0][3] = translation.X;
		data[1][3] = translation.Y;
		data[2][3] = translation.Z;

		return new Matrix(data);
	}

	public static Matrix rotationX(float angle) {
		float[][] data = new float[4][4];
		data[0][0] = 1;
		data[3][3] = 1;
		data[1][1] = (float) Math.cos(angle);
		data[2][2] = (float) Math.cos(angle);
		data[2][1] = (float) Math.sin(angle);
		data[1][2] = (float) Math.sin(-angle);

		return new Matrix(data);
	}

	public static Matrix rotationY(float angle) {
		float[][] data = new float[4][4];
		data[1][1] = 1;
		data[3][3] = 1;
		data[0][0] = (float) Math.cos(angle);
		data[2][2] = (float) Math.cos(angle);
		data[0][2] = (float) Math.sin(angle);
		data[2][0] = (float) Math.sin(-angle);

		return new Matrix(data);
	}

	public static Matrix rotationZ(float angle) {
		float[][] data = new float[4][4];
		data[2][2] = 1;
		data[3][3] = 1;
		data[0][0] = (float) Math.cos(angle);
		data[1][1] = (float) Math.cos(angle);
		data[1][0] = (float) Math.sin(angle);
		data[0][1] = (float) Math.sin(-angle);

		return new Matrix(data);
	}

	public static Matrix scale(Vector3 scale) {
		float[][] data = new float[4][4];
		data[0][0] = scale.X;
		data[1][1] = scale.Y;
		data[2][2] = scale.Z;
		data[3][3] = 1;

		return new Matrix(data);
	}

	public Vector3 getTranslation() {
		return new Vector3(m03, m13, m23);
	}

	public Vector3 getScale() {
		throw new UnsupportedOperationException();
	}

	public Matrix getRotation() {
		float[][] data = getData();
		data[0][3] = 0;
		data[1][3] = 0;
		data[2][3] = 0;
		return new Matrix(data);
	}

	public float getRotationX() {
		throw new UnsupportedOperationException();
	}

	public float getRotationY() {
		throw new UnsupportedOperationException();
	}

	public float getRotationZ() {
		throw new UnsupportedOperationException();
	}

	public float[][] getData() {
		float[][] data = new float[4][4];
		data[0][0] = m00;
		data[0][1] = m01;
		data[0][2] = m02;
		data[0][3] = m03;
		data[1][0] = m10;
		data[1][1] = m11;
		data[1][2] = m12;
		data[1][3] = m13;
		data[2][0] = m20;
		data[2][1] = m21;
		data[2][2] = m22;
		data[2][3] = m23;
		data[3][0] = m30;
		data[3][1] = m31;
		data[3][2] = m32;
		data[3][3] = m33;
		return data;
	}

	public Matrix multiply(Matrix m) {
		float[][] first = this.getData();
		float[][] second = m.getData();
		float[][] mult = new float[4][4];

		float sum = 0;
		for (int c = 0; c < 4; c++) {
			for (int d = 0; d < 4; d++) {
				for (int k = 0; k < 4; k++) {
					sum = sum + first[c][k] * second[k][d];
				}

				if (Math.abs(sum) < 0.01f)
					sum = 0;

				mult[c][d] = sum;
				sum = 0;
			}
		}

		return new Matrix(mult);
	}

	public Matrix getInverseTimesDet() {
		float[][] data = new float[4][4];
		data[0][0] = m11 * m22 * m33 + m12 * m23 * m31 + m13 * m21 * m32 - m11 * m23 * m32 - m12 * m21 * m33 - m13 * m22 * m31;
		data[0][1] = m01 * m23 * m32 + m02 * m21 * m33 + m03 * m22 * m31 - m01 * m22 * m33 - m02 * m23 * m31 - m03 * m21 * m32;
		data[0][2] = m01 * m12 * m33 + m02 * m13 * m31 + m03 * m11 * m32 - m01 * m13 * m32 - m02 * m11 * m33 - m03 * m12 * m31;
		data[0][3] = m01 * m13 * m22 + m02 * m11 * m23 + m03 * m12 * m21 - m01 * m12 * m23 - m02 * m13 * m21 - m03 * m11 * m22;
		data[1][0] = m10 * m23 * m32 + m12 * m20 * m33 + m13 * m22 * m30 - m10 * m22 * m33 - m12 * m23 * m30 - m13 * m20 * m32;
		data[1][1] = m00 * m22 * m33 + m02 * m23 * m30 + m03 * m20 * m32 - m00 * m23 * m32 - m02 * m20 * m33 - m03 * m22 * m30;
		data[1][2] = m00 * m13 * m32 + m02 * m10 * m33 + m03 * m12 * m30 - m00 * m12 * m33 - m02 * m13 * m30 - m03 * m10 * m32;
		data[1][3] = m00 * m12 * m23 + m02 * m13 * m20 + m03 * m10 * m22 - m00 * m13 * m22 - m02 * m10 * m23 - m03 * m12 * m20;
		data[2][0] = m10 * m21 * m33 + m11 * m23 * m30 + m13 * m20 * m31 - m10 * m23 * m31 - m11 * m20 * m33 - m13 * m21 * m30;
		data[2][1] = m00 * m23 * m31 + m01 * m20 * m33 + m03 * m21 * m30 - m00 * m21 * m33 - m01 * m23 * m30 - m03 * m20 * m31;
		data[2][2] = m00 * m11 * m33 + m01 * m13 * m30 + m03 * m10 * m31 - m00 * m13 * m31 - m01 * m10 * m33 - m03 * m11 * m30;
		data[2][3] = m00 * m13 * m21 + m01 * m10 * m23 + m30 * m11 * m20 - m00 * m11 * m23 - m01 * m13 * m20 - m03 * m10 * m21;
		data[3][0] = m10 * m22 * m31 + m11 * m20 * m32 + m12 * m21 * m30 - m10 * m21 * m32 - m11 * m22 * m30 - m12 * m20 * m31;
		data[3][1] = m00 * m21 * m32 + m01 * m22 * m30 + m02 * m20 * m31 - m00 * m22 * m31 - m01 * m20 * m32 - m02 * m21 * m30;
		data[3][2] = m00 * m21 * m31 + m01 * m10 * m32 + m02 * m11 * m30 - m00 * m11 * m32 - m01 * m12 * m30 - m02 * m10 * m31;
		data[3][3] = m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21 - m00 * m12 * m21 - m01 * m10 * m22 - m02 * m11 * m20;

		return new Matrix(data);
	}

	public Matrix getTranspose() {
		float[][] data = new float[4][4];
		float[][] currentData = getData();
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				data[i][j] = currentData[j][i];
			}
		}

		return new Matrix(data);
	}

	public Vector3 multiply(Vector3 v) {
		float x = m00 * v.X + m01 * v.Y + m02 * v.Z + m03;
		float y = m10 * v.X + m11 * v.Y + m12 * v.Z + m13;
		float z = m20 * v.X + m21 * v.Y + m22 * v.Z + m23;
		return new Vector3(x, y, z);
	}

}
